using System;
using System.Drawing;
using FIRADriverLibrary;
using GeometryCSharp;
using System.Collections.Generic;

namespace AGHFiraPlayer
{
	/// <summary>
	/// ball only for now
	/// </summary>
	public abstract class Predicator
	{
		private const double ball_OnColisionVelocityAttenuation = 0.4;

		public static FIRADriver firaDriver;
        public static AGHDriver aghDriver;
		public static RectangleF pitchArea;

		private static Point2D prevBallPosition;
		private static double prevTime;

		private static double vx, vy;
      
		public static void Refresh()
		{
			if (prevTime == firaDriver.time)
				return;

            if (prevBallPosition != null)
            {
                vx = vx * 5 + (firaDriver.ballX - prevBallPosition.x) / (firaDriver.time - prevTime);
                vx /= 6;
                vy = vy * 5 + (firaDriver.ballY - prevBallPosition.y) / (firaDriver.time - prevTime);
                vy /= 6;

                prevBallPosition.x = firaDriver.ballX;
                prevBallPosition.y = firaDriver.ballY;
            }
            else
            {
                prevBallPosition = new Point2D(firaDriver.ballX, firaDriver.ballY);
            }
			
			prevTime = firaDriver.time;
		}

		public static double VX
		{
			get { return vx; }
		}
		public static double VY
		{
			get { return vy; }
		}

		public static Vector2D V
		{
			get { return new Vector2D(vx, vy); }
		}

        public static Boolean isOnCollisionTrajectory(FIRARobotState robot)
        {
            Vector2D ballRobotVector = new Vector2D(robot.x - firaDriver.ballX, robot.y - firaDriver.ballY);
            double angleBallRobot = Vector2D.AngleBetween(Predicator.V, ballRobotVector);
            return Math.Abs(angleBallRobot) < 0.05;
        }

		public static Point2D GetBallPosition(double eta)
		{
			Point2D ballPosition = new Point2D(firaDriver.ballX, firaDriver.ballY);
            //if (detectBallTrajectoryIntersection())
            //    ;
			ballPosition.x += eta * vx;
			ballPosition.y += eta * vy;

			// walls collisions
			bool changed;
			do 
			{
				changed = false;
				if (ballPosition.x < pitchArea.Left)
				{
					ballPosition.x = pitchArea.Left + (pitchArea.Left - ballPosition.x) * ball_OnColisionVelocityAttenuation;
					changed = true;
				}
				if (ballPosition.x > pitchArea.Right)
				{
					ballPosition.x = pitchArea.Right - (ballPosition.x - pitchArea.Right) * ball_OnColisionVelocityAttenuation;
					changed = true;
				}

				if (ballPosition.y < pitchArea.Top)
				{
					ballPosition.y = pitchArea.Top + (pitchArea.Top - ballPosition.y) * ball_OnColisionVelocityAttenuation;
					changed = true;
				}
				if (ballPosition.y > pitchArea.Bottom)
				{
					ballPosition.y = pitchArea.Bottom - (ballPosition.y - pitchArea.Bottom) * ball_OnColisionVelocityAttenuation;
					changed = true;
				}

			}while (changed);   
         
			return ballPosition;
		}

        public static double getA(Point2D p1, Point2D p2)
        {
            return (p1.y - p2.y) / (p1.x - p2.x);
        }

        public static double getB(Point2D p1, Point2D p2)
        {
            return p1.y - getA(p1, p2) * p1.x;
        }

        public static double getBallA()
        {
            double y1 = firaDriver.ballY;
            double y2 = y1 + vy;
            double x1 = firaDriver.ballX;
            double x2 = x1 + vx;
            return (y1 - y2) / (x1 - x2);
        }

        public static double getBallB()
        {
            return firaDriver.ballY - getBallA() * firaDriver.ballX;
        }

        public static double getLineB(double A, double x, double y)
        {
            double b = y - (A * x);
            return b;
        }

        public static double getNormalVectorA(double A)
        {
            double nA = -1 / A;
            return nA;
        }

        public static Point2D getPointFromLines(double A1, double B1, double A2, double B2)
        {
            double x = (B2 - B1) / (A1 - A2);
            double y = A1 * x + B1;
            Point2D P = new Point2D(x,y);
            return P;
        }

        public static Point2D getIntersectionPoint(double x, double y)
        {
            double A1 = getBallA();
            double B1 = getBallB();
            double A2 = getNormalVectorA(A1);
            double B2 = getLineB(A2,x,y);

            Point2D P = getPointFromLines(A1, B1, A2, B2); 
            return P;
        }

        public static Point2D findBestIntersectionPoint(Point2D robotPosition)
        {
            double STEP_MAGNITUDE = AGHDriver.ROBOT_DIAMETER * 2;

            Point2D ballPosition = new Point2D(firaDriver.ballX, firaDriver.ballY);
            double a = getBallA();
            double b = getBallB();
            if (double.IsNaN(a) || double.IsNaN(b) || double.IsInfinity(a) || double.IsInfinity(b)) //the ball is not in motion
                return new Point2D(double.NaN, double.NaN);

            double A2 = getNormalVectorA(a);
            double B2 = getLineB(A2, robotPosition.x, robotPosition.y);

            if (double.IsNaN(A2) || double.IsNaN(B2) || double.IsInfinity(A2) || double.IsInfinity(B2)) //the ball is not in motion
                return new Point2D(double.NaN, double.NaN);

            Point2D intersectionPoint = getPointFromLines(a, b, A2, B2);
            if (boundaryCase(intersectionPoint))
                return new Point2D(double.NaN, double.NaN);

            double neededEta = intersectionPoint.GetDistance(robotPosition) / PotencialDriver.maxVelocity;
            Point2D bestIntersectionPoint = null;
            double step = -Math.Sign(vx);
            if (a > 1)
                step *= Math.Abs(STEP_MAGNITUDE / a);
            else
                step *= STEP_MAGNITUDE;
            double rememberedSign = Math.Sign(intersectionPoint.x - ballPosition.x);

            while (Math.Sign(intersectionPoint.x - GetBallPosition(neededEta).x) == rememberedSign)
            {
                bestIntersectionPoint = new Point2D(intersectionPoint.x, intersectionPoint.y);
                intersectionPoint = new Point2D(a, b, intersectionPoint.x + step);
                neededEta = intersectionPoint.GetDistance(robotPosition) / PotencialDriver.maxVelocity;
            }

            if (bestIntersectionPoint == null)
            {   //znaczy ze nie zdazylibysmy do najblizszego punktu wiec szukamy punktow dalszych
                step = -step;
                int i = 1;
                while (i < 5 && (Math.Sign(intersectionPoint.x - GetBallPosition(neededEta).x) != rememberedSign))
                {
                    intersectionPoint = new Point2D(a, b, intersectionPoint.x + step);
                    neededEta = intersectionPoint.GetDistance(robotPosition) / PotencialDriver.maxVelocity;
                }
                if (i < 4)
                    bestIntersectionPoint = new Point2D(intersectionPoint.x, intersectionPoint.y);
            }

            if (bestIntersectionPoint == null)
                return new Point2D(double.NaN, double.NaN);
            return bestIntersectionPoint;
        }

        private static Boolean boundaryCase(Point2D intersectionPoint)
        {
            //if (!isInPitch(intersectionPoint))
              //  return true;

            if (AGHDriver.home.x == AGHDriver.pitchArea.Left)
            {
                if (firaDriver.ballX < intersectionPoint.x && vx < 0)
                    return true;
                if (firaDriver.ballX > intersectionPoint.x && vx > 0)
                    return true;
            }
            else if (AGHDriver.home.x == AGHDriver.pitchArea.Right)
            {
                if (firaDriver.ballX > intersectionPoint.x && vx > 0)
                    return true;
                if (firaDriver.ballX < intersectionPoint.x && vx < 0)
                    return true;
            }

            return false;
        }

        public static Boolean isInPitch(Point2D location)
        {
            if (location.x < 0 || location.x > pitchArea.Width || location.y < 0 || location.y > pitchArea.Height)
                return false;

            return true;
        }

        public static List<RobotController> getRobotListSortedByDistanceFromGivenPoint(List<RobotController> robotsToChooseFrom, Point2D point)
        {
            List<RobotController> robots = new List<RobotController>();
            for (int i = 0; i < robotsToChooseFrom.Count; i++)
            {
                robots.Add(robotsToChooseFrom[i]);
            }
            robots.Sort(delegate(RobotController r1, RobotController r2) { return r1.Position.GetDistance(point).CompareTo(r2.Position.GetDistance(point)); });

            return robots;
        }

        public static List<RobotController> getRobotListSortedByDistanceFromGivenPoint(Point2D point)
        {
            return getRobotListSortedByDistanceFromGivenPoint(aghDriver.Robots, point);
        }

        public static List<RobotController> getOpponentListSortedByDistanceFromGivenPoint(Point2D point)
        {
            List<RobotController> robots = new List<RobotController>();
            for (int i = 0; i < aghDriver.Oponents.Count; i++)
            {
                robots.Add(aghDriver.Oponents[i]);
            }
            robots.Sort(delegate(RobotController r1, RobotController r2) { return r1.Position.GetDistance(point).CompareTo(r2.Position.GetDistance(point)); });

            return robots;
        }

        public static List<RobotController> getAllRobotListSortedByDistanceFromGivenPoint(Point2D point)
        {
            List<RobotController> robotList = new List<RobotController>();
            robotList.AddRange(aghDriver.Robots);
            robotList.AddRange(aghDriver.Oponents);
            return getRobotListSortedByDistanceFromGivenPoint(robotList, point);
        }
    }
}
